import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.NXTRegulatedMotor;
import lejos.nxt.TouchSensor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.AccelHTSensor;
import lejos.util.Delay;

public class Main {
	static boolean DEBUG = true;

	// Sampling so we get more accurate acceleration values
	static int NUM_SAMPLES = 10;

	static int MAX_MOTOR_SPEED = 20;
	
	// Lower values are faster
	static int LR_ROTATION_SPEED = 15;
	static int UD_ROTATION_SPEED = 20;
	
	static int LR_MIN_POSITION = -90;
	static int LR_MAX_POSITION = 180;
	static int UD_MIN_POSITION = 0;
	static int UD_MAX_POSITION = 420;
	static int FIRING_POSITION = 15;
	
	public static void main(String[] args) {
		AccelHTSensor accel = new AccelHTSensor(SensorPort.S1);
		TouchSensor trigger = new TouchSensor(SensorPort.S2);

		// The accelerometer classes are extremely buggy, but will work much better
		// if we convert all the int values to doubles as soon as we get them.
		double xArray[] = new double[NUM_SAMPLES];
		double yArray[] = new double[NUM_SAMPLES];
		double zArray[] = new double[NUM_SAMPLES];

		// Accumulators for averaging samples
		double xAccum, yAccum, zAccum;

		// Values straight from the accelerometer
		int xAccel, yAccel, zAccel;

		// The final accelerometer values we will use for everything
		double x, y, z;

		NXTRegulatedMotor motorLR = new NXTRegulatedMotor(MotorPort.A);
		NXTRegulatedMotor motorUD = new NXTRegulatedMotor(MotorPort.B);
		NXTRegulatedMotor motorFire = new NXTRegulatedMotor(MotorPort.C);

		motorLR.setSpeed(MAX_MOTOR_SPEED);
		
		int rotateLR;
		int positionLR;
		int rotateUD;
		int positionUD;
		
		boolean hasFired = false;

		int count = 0;
		while(!Button.ESCAPE.isPressed() && !hasFired) {

			// Small delays between accelerometer samples
			Delay.msDelay(20);

			xAccel = accel.getXAccel();
			yAccel = accel.getYAccel();
			zAccel = accel.getZAccel();

			// The Lejos library doesn't seem to understand the accelerometer register
			// values correctly at all, resulting in horribly mangled numbers that make
			// no sense. This logic corrects that as much as possible.
			xArray[count % NUM_SAMPLES] = (xAccel > 200 ? -(1000.0 - xAccel) : xAccel);
			yArray[count % NUM_SAMPLES] = (yAccel > 200 ? -(1000.0 - yAccel) : yAccel);
			zArray[count % NUM_SAMPLES] = (zAccel > 200 ? -(1000.0 - zAccel) : zAccel);

			// Don't actually do anything until we have enough samples
			if(count++ < NUM_SAMPLES) continue;

			xAccum = yAccum = zAccum = 0;
			for(int i = 0; i < NUM_SAMPLES; i++) {
				xAccum += xArray[i];
				yAccum += yArray[i];
				zAccum += zArray[i];
			}

			x = xAccum / NUM_SAMPLES;
			y = yAccum / NUM_SAMPLES;
			z = zAccum / NUM_SAMPLES;

			if(DEBUG) {
				LCD.drawString("X: " + x, 1, 1);
				LCD.drawString("Y: " + y, 1, 2);
				LCD.drawString("Z: " + z, 1, 3);
			}

			// Moving the motors
			positionLR = motorLR.getPosition();
			rotateLR = (int)(y / LR_ROTATION_SPEED);

			// Don't rotate too far
			if(positionLR <= LR_MIN_POSITION && rotateLR < 0) {
				rotateLR = 0;
			}
			if(positionLR >= LR_MAX_POSITION && rotateLR > 0) {
				rotateLR = 0;
			}

			if(rotateLR != 0) {
				motorLR.rotate(-1 * rotateLR, true);
			}
			
			positionUD = motorUD.getPosition();
			rotateUD = -(int)((z / UD_ROTATION_SPEED));
			
			// Try and keep the machine from destroying itself
			if(positionUD <= UD_MIN_POSITION && rotateUD < 0) {
				rotateUD = 0;
			}
			if(positionUD >= UD_MAX_POSITION && rotateUD > 0) {
				rotateUD = 0;
			}

			if(rotateUD != 0) {
				motorUD.rotate(rotateUD, true);
			}
			
			if(DEBUG) {
				LCD.drawString("LR: " + positionLR, 1, 5);
				LCD.drawString("UD: " + positionUD, 1, 6);
			}

			if(trigger.isPressed()) {
				motorFire.rotateTo(FIRING_POSITION);
				
				// Wait a second before wrapping things up
				Delay.msDelay(1000);
				hasFired = true;
			}
		}
		
		// Park motors to ensure consistent zero position
		motorUD.rotateTo(0, true);
		motorFire.rotateTo(0, true);
		motorLR.rotateTo(0);
		
	}
}